#include "linkManager.h"
#include <Eigen/SVD>
#include <time.h>
#include <iostream>
#include <Eigen/Geometry>
#include <omp.h>
#define EPSILON 0.001
#define TOLERANCE 0.01
#define PI 3.14159265
LinkManager::LinkManager(){
}

LinkManager::~LinkManager(){
	for(int i=0; i< (int) links.size(); i++){
		if(links[i]!=NULL)
			delete links[i];
	}
}
void LinkManager::initLink(Vector3f o, Vector3f axis, float t, float l){
	origin = o;
	Link* ini = new Link(Vector3f(0, 0, 0), axis, t, l);
	ini->tMatrix = getComposedMatrix(axis, t, l);
	ini->end = getLinkEnd(axis, t, l, links.size()) ;
	ini->origin = o;
	links.push_back(ini);
}


void LinkManager::addLink(Vector3f axis, float t,  float l){
	int size = links.size();

	Link* new_link = new Link(links[size-1]->end, axis, t, l);

	new_link->tMatrix.setIdentity();
	for(int i=0; i< (int) links.size(); i++){
		new_link->tMatrix *= getComposedMatrix(links[i]->axis, links[i]->theta, links[i]->length);
	}	
	new_link->tMatrix *= getComposedMatrix(axis, t, l);

	new_link->end = getPos(new_link, Vector3f(0, 0, 0));
	new_link->origin = origin;
	links.push_back(new_link);	
}

void LinkManager::display(){
	for(int i=0; i< (int) links.size(); i++)
		links[i]->draw();
}
Vector3f LinkManager::getLinkEnd(Vector3f axis, float t, float l, int linkNum){
	Matrix<float, 4, 4> composed_matrix;

	composed_matrix.setIdentity();
	for(int i=0; i<linkNum; i++){
		composed_matrix *= links[i]->tMatrix;	
	}	

	composed_matrix *= getComposedMatrix(axis, t, l);

	Vector4f vec = composed_matrix* Vector4f(0, 0, 0, 1);

	return Vector3f(vec[0], vec[1], vec[2]);
}

Vector3f LinkManager::getPos(Link* l, Vector3f v){
	
	Vector4f vec = 	l->tMatrix * Vector4f(v(0), v(1), v(2), 1);

	return Vector3f(vec[0], vec[1], vec[2]);
}
Matrix<float, 4, 4> LinkManager::getT(float length){
	Matrix<float, 4, 4> T;
	T.setZero(4, 4);
	T(0, 0) = 1;
	T(1, 1) = 1;
	T(2, 2) = 1;
	T(3, 3) = 1;
	T(0, 3) = length;
	T(1, 3) = 0;
	T(2, 3) = 0;
	return T;
}
Matrix<float, 4, 4> LinkManager::getComposedMatrix(Vector3f axis, float theta,  float length){

	Matrix<float, 4, 4> T;
	T = getT(length);

	Quaternionf q;
	q = AngleAxis<float>(theta,axis);

	Matrix<float, 3, 3> R = q.toRotationMatrix();
	
	Matrix<float, 4, 4> R_h;
	R_h.setIdentity();
	for(int i = 0; i<3; i++)
		for(int j = 0; j<3; j++)
			R_h(i, j) = R(i, j);

	Matrix<float, 4, 4> composed;
	composed.setIdentity();

	composed = R_h * T;

	return composed;

}



void LinkManager::calculate(Vector3f end){
	
	srand(time(NULL));
	bool move = false;
	Matrix<float, 12, 1> psi;
	psi(0,0) = links[0]->theta;
	psi(1,0) = links[0]->axis[0];
	psi(2,0) = links[0]->axis[1];
	psi(3,0) = links[0]->axis[2];

	psi(4,0) = links[1]->theta; 
	psi(5,0) = links[1]->axis[0];
	psi(6,0) = links[1]->axis[1];
	psi(7,0) = links[1]->axis[2];

	psi(8,0) = links[2]->theta;
	psi(9,0) = links[2]->axis[0];
	psi(10,0) = links[2]->axis[1];
	psi(11,0) = links[2]->axis[2];

	Vector3f ori = links[0]->pos;


	float len = links[0]->length + links[1]->length + links[2]->length - EPSILON;
	if((end-ori).norm() > len){
		end = ori + (end - ori) * (len / (end-ori).norm()); 
	}
	
	while((pi(psi, end)).norm()>=TOLERANCE) {
	
		MatrixXf m = MatrixXf::Random(3,12);
		Matrix<float, 3, 12> j = get3D3LJacobian(psi);
		JacobiSVD<MatrixXf> svd(m);
		svd.compute(j, ComputeFullU|ComputeFullV);
	    Matrix3f u = svd.matrixU();
		Matrix<float, 12, 12> v = svd.matrixV();
		Vector3f s = svd.singularValues();
	
		Matrix3f u_trans = u.transpose();

		Matrix<float, 12, 3> s_inverse;
		s_inverse.setZero(12, 3);
		for (int i = 0; i < 3; i++) {
			if(fabs(s(i)) > EPSILON)
				s_inverse(i, i) = 1/s(i);
		}
		
		Matrix<float, 12, 3> T = v * s_inverse * u_trans;

		psi = psi - T*pi(psi, end); 
		move = true;
	}
	if(move){
		//omp_set_num_threads(4);	
		Matrix<float, 4, 4> trans;
		trans.setIdentity();
			
		for(int i=0; i< (int) links.size(); i++){
			float theta = psi[i*4];
			float x = psi[i*4+1];
			float y = psi[i*4+2];
			float z = psi[i*4+3];
			Vector3f axis(x, y, z);
			axis.normalize();
			trans = trans * getComposedMatrix(axis, theta, links[i]->length) ;
			links[i]->theta = theta;
			links[i]->tMatrix = trans;
			links[i]->axis = axis;
			/*	links[i]->end = getPos(links[i], Vector3f(0 ,0 ,0));
						if(i!= (links.size()-1))
				links[i+1]->pos = links[i]->end;
				*/
			//#pragma omp parallel for		
			for(int j=0; j< (int) links[i]->vertices.size(); j++){
				triple* temp = links[i]->vertices[j];
				Vector3f local = temp->myVertexLocal;
				Vector4f temp_vec(local(0), local(1), local(2), 1);
				temp_vec = links[i]->tMatrix * temp_vec;
				temp->myVertex = Vector3f(temp_vec(0), temp_vec(1), temp_vec(2)) + origin;

			}
		}
	}
	
}

Vector3f LinkManager::pi(const Matrix<float, 12, 1>& psi, Vector3f g){
	return p(psi) - g;
}

Vector3f LinkManager:: p(const Matrix<float, 12, 1>& psi) {
	Matrix<float ,  4, 4> composedMatrix;
	composedMatrix.setIdentity();

	for(int i=0; i< (int) links.size(); i++){
		float theta = psi[i*4];
		float x = psi[i*4+1];
		float y = psi[i*4+2];
		float z = psi[i*4+3];
		Vector3f axis;

		

		if(i!=0){
			Vector3f w = links[i-1]->end - links[i-1]->pos;
			Vector3f v = Vector3f(x, y, z);

			axis = v - v.dot(w) * w / w.norm();

			/*x = 0;
			Vector4f axis_h(x, y, z, 1);
			Matrix<float, 4, 4> t_inverse = links[i-1]->tMatrix.inverse();
			axis_h = t_inverse * axis_h;
			axis = Vector3f(axis_h(0), axis_h(1), axis_h(2));*/
		}else
			axis = Vector3f(x, y, z);
		
		axis.normalize();
		Matrix<float, 4, 4> trans =  getComposedMatrix(axis, theta, links[i]->length);
		
		links[i]->end = getPos(links[i], Vector3f(0 ,0 ,0));

		if(i!= ((int) links.size()-1))
				links[i+1]->pos = links[i]->end;


		composedMatrix *= trans;
	}
	
	Vector4f vec = composedMatrix* Vector4f(0, 0, 0, 1);
	return Vector3f(vec[0], vec[1], vec[2]);
}

Matrix<float, 3, 12> LinkManager::get3D3LJacobian(const Matrix<float, 12, 1>& psi){

	Matrix<float,3,12> m;
	Matrix<float, 12, 1> psi_temp;

	//psi_temp(0,0) = psi_temp(0,0) + EPSILON;
	//omp_set_num_threads(4);
	for (int i = 0; i < 3; i ++) {
		psi_temp = psi;
		//#pragma parallel for
		for(int j= 0; j<12; j++){
		psi_temp(j,0) = psi_temp(j,0) + EPSILON;
		m(i,j) = (p(psi_temp)[i] - p(psi)[i]) / EPSILON;
		psi_temp(j,0) = psi_temp(j,0) - EPSILON;
		}
}

		
	return m;
}
